#include "icm20602.h"

#include <math.h>
#include <stdbool.h>

#include "../bsp/bsp_spi.h"
#include "../bsp/bsp_tim.h"

static bool ICM_GyroStable(RawFloat_t *raw, RawFloat_t *raw_last) {
  return ((fabs(raw->gyro.x - raw_last->gyro.x) < 0.3f) &&
          (fabs(raw->gyro.y - raw_last->gyro.y) < 0.3f) &&
          (fabs(raw->gyro.z - raw_last->gyro.z) < 0.3f) &&
          (fabs(raw->acc.y - raw_last->acc.y) < 0.3f) &&
          (fabs(raw->acc.z - raw_last->acc.z) < 0.3f) &&
          (fabs(raw->acc.x - raw_last->acc.x) < 0.3f));
}

void ICM_SelfCheck(void) {
  uint8_t dat = 0;
  while (0x12 != dat) {
    Bsp_SPI_ReadByte(ICM20602_WHO_AM_I, &dat);
    BSP_Delay(10);
  }
}

/**
 * @brief 初始化ICM
 *
 */
void ICM_init(void) {
  uint8_t val = 0x0;

  BSP_Delay(10);

  ICM_SelfCheck();
  /* 复位 */
  Bsp_SPI_WriteByte(ICM20602_PWR_MGMT_1, 0x80);
  BSP_Delay(2);
  /* 复位成功 */
  do {
    Bsp_SPI_ReadByte(ICM20602_PWR_MGMT_1, &val);
  } while (0x41 != val);

  Bsp_SPI_WriteByte(ICM20602_PWR_MGMT_1, 0x01);
  /* 时钟设置 */
  Bsp_SPI_WriteByte(ICM20602_PWR_MGMT_2, 0x00);
  /* 开启陀螺仪和加速度计 */
  Bsp_SPI_WriteByte(ICM20602_CONFIG, 0x04);
  /*  176HZ 1KHZ */
  Bsp_SPI_WriteByte(ICM20602_SMPLRT_DIV, 0x00);
  /* 采样速率 SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1
           + SMPLRT_DIV) */
  Bsp_SPI_WriteByte(ICM20602_GYRO_CONFIG, 0x00);
  /* ±2000 dps */
  Bsp_SPI_WriteByte(ICM20602_ACCEL_CONFIG, 0x18);
  /* ±8g */
  Bsp_SPI_WriteByte(ICM20602_ACCEL_CONFIG_2, 0x03);
  /*  Average 4 samples   44.8HZ */
  /* 0x23 Average 16 */
  /* ：°/s
      ICM20602_GYRO_CONFIG
      1.0x00 陀螺仪量程为:±250 dps  除以131
      2.0x08 陀螺仪量程为:±500 dps  除以65.5
      3.0x10 陀螺仪量程为:±1000dps  除以32.8
      4.0x18 陀螺仪量程为:±2000dps  除以16.4
  */
  /*  g(m/s^2)
       ICM20602_ACCEL_CONFIG
      0x00 加速度计量程为:±2g  除以16384
      0x08 加速度计量程为:±4g  除以8192
      0x10 加速度计量程为:±8g  除以4096
      0x18 加速度计量程为:±16g 除以2048
 */
}

void ICM_GetAcc(RawICM_t *raw) {
  struct {
    uint8_t reg;
    uint8_t dat[6];
  } buf;

  buf.reg = ICM20602_ACCEL_XOUT_H | ICM20602_SPI_R;

  Bsp_SPI_ReadBytes(&buf.reg, 7);
  raw->acc.x = (int16_t)(((uint16_t)buf.dat[0] << 8 | buf.dat[1]));
  raw->acc.y = (int16_t)(((uint16_t)buf.dat[2] << 8 | buf.dat[3]));
  raw->acc.z = (int16_t)(((uint16_t)buf.dat[4] << 8 | buf.dat[5]));
}

void ICM_GetGyro(RawICM_t *raw) {
  struct {
    uint8_t reg;
    uint8_t dat[6];
  } buf;

  buf.reg = ICM20602_GYRO_XOUT_H | ICM20602_SPI_R;

  Bsp_SPI_ReadBytes(&buf.reg, 7);
  raw->gyro.x = (int16_t)(((uint16_t)buf.dat[0] << 8 | buf.dat[1]));
  raw->gyro.y = (int16_t)(((uint16_t)buf.dat[2] << 8 | buf.dat[3]));
  raw->gyro.z = (int16_t)(((uint16_t)buf.dat[4] << 8 | buf.dat[5]));
}
/* ：°/s
    ICM20602_GYRO_CONFIG
    1.0x00 陀螺仪量程为:±250 dps  除以131
    2.0x08 陀螺仪量程为:±500 dps  除以65.5
    3.0x10 陀螺仪量程为:±1000dps  除以32.8
    4.0x18 陀螺仪量程为:±2000dps  除以16.4
*/
/*  g(m/s^2)
     ICM20602_ACCEL_CONFIG
    0x00 加速度计量程为:±2g  除以16384
    0x08 加速度计量程为:±4g  除以8192
    0x10 加速度计量程为:±8g  除以4096
    0x18 加速度计量程为:±16g 除以2048
    0.886 - 0.547 - 0.243 + 0.133 - 0.974 + 0.098
*/
RawFloat_t ICM_Int16toFloat(RawICM_t *raw,Cali_t *cali) {
  RawFloat_t rawf;
  rawf.gyro.x = (float)raw->gyro.x / 131 - cali->icm20602.gyro_offset.x;
  rawf.gyro.y = (float)raw->gyro.y / 131 - cali->icm20602.gyro_offset.y;
  rawf.gyro.z = (float)raw->gyro.z / 131 - cali->icm20602.gyro_offset.z;

  rawf.acc.x = (float)raw->acc.x / 2048 - cali->icm20602.acc_offset.x;
  rawf.acc.y = (float)raw->acc.y / 2048- cali->icm20602.acc_offset.y;
  rawf.acc.z = (float)raw->acc.z / 2048- cali->icm20602.acc_offset.z;

  return rawf;
}
void ICM_Cali(Cali_t *cali, RawICM_t *raw) {
  uint16_t count = 0;
  static float x = 0.0f;
  static float y = 0.0f;
  static float z = 0.0f;
  static float a = 0.0f;
  static float b = 0.0f;
  static float c = 0.0f;

  static RawICM_t *raw_l;
  ICM_GetGyro(raw_l);
  ICM_GetAcc(raw);
  RawFloat_t raw_last = ICM_Int16toFloat(raw_l,cali);
  while (count < 10000) {
    ICM_GetGyro(raw);
    ICM_GetAcc(raw);
    RawFloat_t raw_t = ICM_Int16toFloat(raw,cali);
    bool data_good = ICM_GyroStable(&raw_t, &raw_last);
    if (1) {
      x += raw_t.gyro.x;
      y += raw_t.gyro.y;
      z += raw_t.gyro.z;
      a += raw_t.acc.x;
      b += raw_t.acc.y;
      c += raw_t.acc.z;

      count++;
    }
    raw_last = raw_t;
  }
  cali->icm20602.gyro_offset.x = x / (float)count;
  cali->icm20602.gyro_offset.y = y / (float)count;
  cali->icm20602.gyro_offset.z = z / (float)count;
  cali->icm20602.acc_offset.x = a / (float)count;
  cali->icm20602.acc_offset.y = b / (float)count;
  cali->icm20602.acc_offset.z = c / (float)count;
}
